// menu.c
#include "menu.h"
#include "pid.h"
#include <math.h>

// 页面状态
uint8_t current_page = 0; // 0: 主页面，1: PID选择页面，2: PID参数选择页面
uint8_t selected_pid = 0; // 选择的PID控制器，0: 左右，1: 前后，2: 转向, 3: 速度, 4: Error
uint8_t selected_param = 0; // 选择的PID参数，0: Kp，1: Ki，2: Kd
uint8_t selected_loop = 0; // 选择的环，0: 速度环, 1: 角度环, 2: 角速度环
uint8_t adjusting_mode = 0; // 0: 选择模式，1: 调整模式

// 上一次显示的值，用于检测变化
static float last_value = 0.0f;
static uint8_t last_adjusting_mode = 0;
static uint8_t last_selected_pid = 0;
static uint8_t last_selected_param = 0;
static uint8_t last_selected_loop = 0;

// 用于记录页面切换
static uint8_t last_page = 0xFF; // 初始值设为非法值，确保第一次会刷新

// PID参数显示位置
#define START_Y 30 // 从图像上方开始显示
#define LINE_SPACING 15 // 行间距

// PID控制器名称数组（拼音）
static const char* pid_names[5] = {"ZuoYou", "QianHou", "Zhuan", "Speed", "Error"};
static const char* loop_names[3] = {"Speed Loop", "Angle Loop", "AngleSpeed Loop"};
static const char* param_names[3] = {"KP", "KI", "KD"};

void clear_screen(void) {
    ips200_full(RGB565_WHITE); // 用白色填充整个屏幕
    // 重置颜色为默认值
    ips200_set_color(IPS200_DEFAULT_PENCOLOR, IPS200_DEFAULT_BGCOLOR);
}

void clear_text_area(uint16_t x, uint16_t y, uint16_t width, uint16_t height) {
    for (uint16_t j = y; j < y + height; j++) {
        for (uint16_t i = x; i < x + width; i++) {
            ips200_draw_point(i, j, RGB565_WHITE);
        }
    }
}

void key_main(void) {
    // KEY3 功能 (短按/长按)
    if (key_get_state(KEY_3) != 0) {
        if (key_get_state(KEY_3) == 1) { // 短按
            if (current_page == 0) {
                // 主页面：切换run状态
                run = !run;
            } else if (current_page == 1) {
                // PID选择页面：进入参数选择页面
                current_page = 2;
                adjusting_mode = 0; // 重置调整模式
                selected_loop = 0; // 默认选择速度环
                selected_param = 0; // 默认选择KP参数
            } else if (current_page == 2) {
                // 参数选择页面：切换调整模式
                adjusting_mode = !adjusting_mode;
            }
        } else if (key_get_state(KEY_3) == 2) { // 长按
            if (current_page == 0) {
                // 主页面：切换go_flag状态
                go_flag = !go_flag;
            } else if (current_page == 1) {
                // PID菜单：返回主页面
                current_page = 0;
                adjusting_mode = 0;
                clear_screen();
            } else if (current_page == 2) {
                // 参数选择页面：返回PID选择页面
                current_page = 1;
                adjusting_mode = 0;
                clear_screen();
            }
        }
        key_clear_state(KEY_3);
    }

    // PID菜单导航
    if (current_page == 1) {
        // PID选择页面
        if (key_get_state(KEY_2) == 1) { // 短按
            selected_pid = (selected_pid + 1) % 5;
            key_clear_state(KEY_2);
        }
        if (key_get_state(KEY_4) == 1) { // 短按
            selected_pid = (selected_pid + 4) % 5; // 相当于减1
            key_clear_state(KEY_4);
        }

    } else if (current_page == 2) {
        if (selected_pid == 3 || selected_pid == 4) {
            // 速度或Error调整
            if (key_get_state(KEY_2) == 1) { // 短按
                if (adjusting_mode) {
                    adjust_pid_value(1);
                }
                key_clear_state(KEY_2);
            }
            if (key_get_state(KEY_4) == 1) { // 短按
                if (adjusting_mode) {
                    adjust_pid_value(-1);
                }
                key_clear_state(KEY_4);
            }
        } else {
            // 参数选择页面
            if (key_get_state(KEY_2) == 1) { // 短按
                if (!adjusting_mode) {
                    // 选择模式下：切换参数或环
                    if (selected_param < 2) {
                        selected_param++;
                    } else {
                        selected_param = 0;
                        if (selected_loop < 2) {
                            selected_loop++;
                        } else {
                            selected_loop = 0;
                        }
                    }
                } else {
                    // 调整模式下：增加参数值
                    adjust_pid_value(1);
                }
                key_clear_state(KEY_2);
            }
            if (key_get_state(KEY_4) == 1) { // 短按
                if (!adjusting_mode) {
                    // 选择模式下：切换参数或环
                    if (selected_param > 0) {
                        selected_param--;
                    } else {
                        selected_param = 2;
                        if (selected_loop > 0) {
                            selected_loop--;
                        } else {
                            selected_loop = 2;
                        }
                    }
                } else {
                    // 调整模式下：减少参数值
                    adjust_pid_value(-1);
                }
                key_clear_state(KEY_4);
            }
        }
    } else {
        // 主页面按键处理
        if (key_get_state(KEY_4) == 1) { // 短按
            current_page = 1; // 进入PID选择页面
            clear_screen();
            key_clear_state(KEY_4);
        }
    }
}

void adjust_pid_value(int8_t direction) {
    // 根据不同的PID控制器、环和参数设置不同的步进值
    float step = 0.1f; // 默认步进值

    // 速度调整
    if (selected_pid == 3) {
        bdc_speed1 += direction * 10.0f; // 步进值10
        return;
    }

    // Error调整
    if (selected_pid == 4) {
        regular_error += direction * 1; // 步进值1
        return;
    }

    // 根据选择的PID控制器、环和参数设置步进值
    switch (selected_pid) {
        case 0: // 左右平衡
            switch (selected_loop) {
                case 0: // 速度环
                    if (selected_param == 0 || selected_param == 1) step = 0.0001f;
                    break;
                case 1: // 角度环
                    if (selected_param == 0) step = 0.1f;
                    else if (selected_param == 2) step = 1.0f;
                    break;
                case 2: // 角速度环
                    if (selected_param == 0) step = 1.0f;
                    else if (selected_param == 1) step = 0.1f;
                    break;
            }
            break;
        case 1: // 前后平衡
            switch (selected_loop) {
                case 0: // 速度环
                    if (selected_param == 0 || selected_param == 1) step = 0.001f;
                    break;
                case 1: // 角度环
                    if (selected_param == 0) step = 0.1f;
                    else if (selected_param == 2) step = 1.0f;
                    break;
                case 2: // 角速度环
                    if (selected_param == 0) step = 1.0f;
                    else if (selected_param == 1) step = 0.01f;
                    break;
            }
            break;
        case 2: // 转向
            switch (selected_loop) {
                case 0: // 速度环
                    if (selected_param == 0) step = 0.01f;
                    break;
                case 1: // 角度环
                    if (selected_param == 0) step = 0.1f;
                    else if (selected_param == 2) step = 1.0f;
                    break;
                case 2: // 角速度环
                    if (selected_param == 0) step = 1.0f;
                    break;
            }
            break;
    }

    PID_IncTypeDef* pid_ptr = NULL;

    // 根据选择的PID控制器和环获取指针
    switch (selected_pid) {
        case 0: // 左右
            switch (selected_loop) {
                case 0: pid_ptr = &roll_speed_wheel; break;     // 速度环
                case 1: pid_ptr = &roll_angle_wheel; break;     // 角度环
                case 2: pid_ptr = &roll_anglespeed_wheel; break; // 角速度环
            }
            break;
        case 1: // 前后
            switch (selected_loop) {
                case 0: pid_ptr = &bdc_speed_wheel; break;      // 速度环
                case 1: pid_ptr = &bdc_banlance_wheel; break;   // 角度环
                case 2: pid_ptr = &bdc_wheel; break;            // 角速度环
            }
            break;
        case 2: // 转向
            switch (selected_loop) {
                case 0: pid_ptr = &bldc_yaw_speed_wheel; break;      // 速度环
                case 1: pid_ptr = &bldc_yaw_angle_wheel; break;     // 角度环
                case 2: pid_ptr = &bldc_yaw_anglespeed_wheel; break; // 角速度环
            }
            break;
    }

    if (pid_ptr) {
        // 根据选择的参数调整值
        switch (selected_param) {
            case 0: // Kp
                pid_ptr->Kp += direction * step;
                break;
            case 1: // Ki
                pid_ptr->Ki += direction * step;
                break;
            case 2: // Kd
                pid_ptr->Kd += direction * step;
                break;
        }
    }
}

void menu(void) {
    // 如果页面切换，清屏并重置状态
    if (last_page != current_page) {
        clear_screen();
        last_page = current_page;
        // 重置上一次记录的状态，以便强制更新
        last_adjusting_mode = 0xFF;
        last_selected_pid = 0xFF;
        last_selected_param = 0xFF;
        last_selected_loop = 0xFF;
        last_value = -1000.0f;
    }

    if (current_page == 0) {
        // 主页面显示
        ips200_show_float(10, 130, eulerAngle.yaw, 4, 6);
        ips200_show_float(10, 150, eulerAngle.pitch, 4, 6);
        ips200_show_float(10, 170, eulerAngle.roll, 4, 6);
        ips200_show_float(110, 150, image_error, 4, 6);
        ips200_show_float(110, 190, speed_Pwm, 4, 6);
        ips200_show_float(110, 210, PWM_out, 4, 6);
        ips200_show_float(110, 230, Pwm, 4, 6);
        ips200_show_float(10, 190, encoder_bdc, 4, 6);
        ips200_show_float(10, 210, Dynamic_zero, 4, 6);
        ips200_show_float(10, 230, xxxx, 4, 6);

        ips200_show_string(10, 250, "go_flag");
        ips200_show_int(70, 250, go_flag, 4);
        ips200_show_string(10, 270, "run");
        ips200_show_int(70, 270, run, 4);

        ips200_show_string(120, 250, "biao_x");
        ips200_show_int(170, 250, biao_x, 4);
        ips200_show_string(120, 270, "biao_y");
        ips200_show_int(170, 270, biao_y, 4);
        ips200_show_string(120, 290, "labelcount");
        ips200_show_int(200, 290, labelcount, 4);
        ips200_show_string(10, 290, "area");
        ips200_show_int(70, 290, area, 4);

        // 显示速度值
        ips200_show_string(110, 170, "Speed:");
        ips200_show_int(160, 170, (int)bdc_speed1, 4);

        ips200_show_gray_image(0, 0, (uint8*)ErZhiHua_Image1[0], MT9V03X_W, MT9V03X_H, MT9V03X_W, MT9V03X_H, 0);
        eye_show();
    } else if (current_page == 1) {
        // PID选择页面
        char display_str[50];
        // 显示标题
        ips200_show_string(10, 130, "Select PID:");
        // 显示所有PID控制器，当前选中的高亮或标记
        for (int i = 0; i < 5; i++) {
            if (i == selected_pid) {
                sprintf(display_str, "> %s", pid_names[i]);
            } else {
                sprintf(display_str, "  %s", pid_names[i]);
            }
            ips200_show_string(10, 130 + (i+1)*LINE_SPACING, display_str);
        }

    } else if (current_page == 2) {
        // PID参数选择页面
        char display_str[50];
        int y_pos = START_Y;

        // 显示标题 - PID名称
        sprintf(display_str, "PID: %s", pid_names[selected_pid]);
        ips200_show_string(10, y_pos, display_str);
        y_pos += LINE_SPACING;

        if (selected_pid == 3 || selected_pid == 4) {
            // 速度或Error调整
            float value = (selected_pid == 3) ? bdc_speed1 : regular_error;
            clear_text_area(60, y_pos, 60, LINE_SPACING); // 清除旧数值区域
            sprintf(display_str, "Value: %d", (int)value);
            if (adjusting_mode) {
                ips200_show_string(10, y_pos, "> ");
                ips200_show_string(20, y_pos, display_str);
            } else {
                ips200_show_string(10, y_pos, display_str);
            }
        } else {
            // 显示三个环的参数
            for (int loop = 0; loop < 3; loop++) {
                // 显示环名称
                if (loop == selected_loop) {
                    sprintf(display_str, "> %s", loop_names[loop]);
                } else {
                    sprintf(display_str, "  %s", loop_names[loop]);
                }
                ips200_show_string(10, y_pos, display_str);
                y_pos += LINE_SPACING;

                // 获取当前环的PID指针
                PID_IncTypeDef* pid_ptr = NULL;
                switch (selected_pid) {
                    case 0: // 左右
                        switch (loop) {
                            case 0: pid_ptr = &roll_speed_wheel; break;
                            case 1: pid_ptr = &roll_angle_wheel; break;
                            case 2: pid_ptr = &roll_anglespeed_wheel; break;
                        }
                        break;
                    case 1: // 前后
                        switch (loop) {
                            case 0: pid_ptr = &bdc_speed_wheel; break;
                            case 1: pid_ptr = &bdc_banlance_wheel; break;
                            case 2: pid_ptr = &bdc_wheel; break;
                        }
                        break;
                    case 2: // 转向
                        switch (loop) {
                            case 0: pid_ptr = &bldc_yaw_speed_wheel; break;
                            case 1: pid_ptr = &bldc_yaw_angle_wheel; break;
                            case 2: pid_ptr = &bldc_yaw_anglespeed_wheel; break;
                        }
                        break;
                }

                if (pid_ptr) {
                    // 显示三个参数
                    for (int param = 0; param < 3; param++) {
                        float value = 0.0;
                        switch (param) {
                            case 0: value = pid_ptr->Kp; break;
                            case 1: value = pid_ptr->Ki; break;
                            case 2: value = pid_ptr->Kd; break;
                        }

                        if (loop == selected_loop && param == selected_param) {
                            // 当前选中的参数
                            sprintf(display_str, "  > %s: %.4f", param_names[param], value);
                        } else {
                            sprintf(display_str, "    %s: %.4f", param_names[param], value);
                        }
                        ips200_show_string(10, y_pos, display_str);
                        y_pos += LINE_SPACING;
                    }
                }
            }
        }

        // 显示模式
        y_pos += LINE_SPACING;
        ips200_show_string(10, y_pos, "Mode:");
        if (adjusting_mode) {
            ips200_show_string(60, y_pos, "change");
        } else {
            ips200_show_string(60, y_pos, "xuanze");
        }

    }
}
